#include "angular.h"
#include <webots/motor.h>
#include "my_controller_Luntui.h"

// //编码器微分函数
// double R_position_past = 0.0;
// double L_position_past = 0.0;
// int revolution_count = 0;  // 改进变量命名

// void calculate_angular_velocity(chassis_t *chassis,double R_current,double L_current) {
//     const double PI = 3.14159265358979323846;  // 明确定义π值

//     // 计算位置差（改进数值稳定性）
//     double delta_R = R_current - R_position_past;
//     double delta_L = L_current - L_position_past;

//     // // 方向检测与圈数计数（优化逻辑）
//     // if (delta_R < 0) {
//     //     revolution_count++;  // 逆时针方向计数
//     //     delta_R += 2 * PI;    // 补偿负向差值
//     // } else {
//     //     // 自动处理正向方向差值（无需额外操作）
//     // }

//     // // 计算总角度（保留完整圈数信息）
//     // double R_total = revolution_count * 2 * PI + R_current;
//     // double L_total = revolution_count * 2 * PI + L_current;

//     // 计算角速度（使用更精确的浮点除法）
//     chassis->wheel_motor[0].vel = delta_R / TIME_step_s;
//     chassis->wheel_motor[1].vel = delta_L / TIME_step_s;

//     // printf("速度: %.3f /n",chassis->wheel_motor[0].vel);

//     // 更新历史位置（使用总角度进行存储）
//     R_position_past = R_total;
//     L_position_past = L_total;
// }
